//
// This file is part of cpp-lib, a library of C++ functions and classes.
// Cpp-lib is copyright (C) 2004 and onwards, The CPL Interest Group.
// 
// This library is free software; you can redistribute it and/or modify it
// under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or (at
// your option) any later version.
// 
// This library is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
// General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public License
// along with this library (see the file COPYING); if not, write to the
// Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
// 02111-1307  USA
//
// $Id$
//

//
// Test harness for quaternions and geometry.
//

#include <iostream>
#include <limits>

#include "cpp-lib/matrix.h"
#include "cpp-lib/matrix-io.h"
#include "cpp-lib/quaternion.h"
#include "cpp-lib/geometry.h"
#include "cpp-lib/math-util.h"

using namespace cpl::math ;

typedef matrix< double , fixed< 3 , 3 > > mat_3_t ;
typedef matrix< double , fixed< 3 , 1 > > vec_3_t ;

vec_3_t const
random_vector() {

  vec_3_t ret ;
  ret( 1 ) = std::rand() ;
  ret( 2 ) = std::rand() ;
  ret( 3 ) = std::rand() ;

  return ret ;

}


vec_3_t const
random_unit_vector() {

  vec_3_t v = random_vector() ;
  return 1 / norm_2( v ) * v ;

}



quaternion<> const random_unit_quaternion() {

  quaternion<> ret( std::rand() , std::rand() , std::rand() , std::rand() ) ;
  normalize( ret ) ;
  return ret ;

}


// Interactively test rotation

void interactive() {

  vec_3_t a ;
  double theta ;
  long n ;

  while( 1 ) {
    std::cout << "enter theta (degrees), axis, n: " ;
    std::cin  >> theta >> a( 1 ) >> a( 2 ) >> a( 3 ) >> n ;

    theta *= pi / 180 ;

    if( !std::cin ) return ;

    quaternion<> q = rotation_quaternion( theta , a ) ;
    vec_3_t v = random_unit_vector() ;

    for( long i = 0 ; i < n ; ++i ) {

      std::cout << v ; 
      v = rotation( q , v ) ;

    }

  }

}





mat_3_t const yaw_pitch_roll( euler_angles<> const& ea ) {

  double const cpsi   = std::cos( ea.psi   ) ;
  double const spsi   = std::sin( ea.psi   ) ;
  double const ctheta = std::cos( ea.theta ) ;
  double const stheta = std::sin( ea.theta ) ;
  double const cphi   = std::cos( ea.phi   ) ;
  double const sphi   = std::sin( ea.phi   ) ;

  mat_3_t yaw , pitch , roll ;

  yaw( 1 , 1 ) =  cpsi ; yaw( 1 , 2 ) = spsi ;
  yaw( 2 , 1 ) = -spsi ; yaw( 2 , 2 ) = cpsi ;
  yaw( 3 , 3 ) = 1 ;

  pitch( 1 , 1 ) = ctheta ; pitch( 1 , 3 ) = -stheta ;
  pitch( 3 , 1 ) = stheta ; pitch( 3 , 3 ) =  ctheta ;
  pitch( 2 , 2 ) = 1 ;

  roll( 2 , 2 ) =  cphi ; roll( 2 , 3 ) = sphi ;
  roll( 3 , 2 ) = -sphi ; roll( 3 , 3 ) = cphi ;
  roll( 1 , 1 ) = 1 ;

  return roll * mat_3_t( pitch * yaw ) ; // Explicit conversion due to ET!

}


//
// Return ``deviation'' of matrix from an orthogonal matrix.
//

double const ortho_dev( mat_3_t const& C ) {

  double ret = 0 ;

  for( long i = 1 ; i < C.rows() ; ++i ) {
  for( long j = i ; j < C.rows() ; ++j ) {

	double const d = 
	  std::fabs( ( i == j ) - ( transpose( rows( C , i , i ) ) | transpose( rows( C , j , j ) ) ) ) ;

	ret = std::max( ret , d ) ;

  }
  }

  return ret ;

}



int main() {

  std::cout << "Test harness for quaternions and geometry.\n" ;

  // interactive() ;

  std::cout << "Testing sphere_surface_frame().\n" ;
  
  double v_res_max = 0 ;
  
  for( int i = 0 ; i < 10000 ; ++i ) {
    
	vec_3_t x = random_vector() ;
	mat_3_t const C = sphere_surface_frame( x ) ;

	// Check deviation from orthogonal matrix.
	v_res_max = std::max( ortho_dev( C ) , v_res_max ) ;
	// Check deviation from det = 1.
	v_res_max = std::max( std::fabs( 1 - det_3( C ) ) , v_res_max ) ;

  }
  
  std::cout << "max residual: "
	        << v_res_max << '\n' ;


  std::cout << "Testing spherical <-> cartesian coordinate conversion.\n" ;
  
  v_res_max = 0 ;

  for( int i = 0 ; i < 10000 ; ++i ) {

	double r ;
	double theta ;
	double phi ;

    vec_3_t x = random_vector() ;

	cartesian_to_spherical( x , r , theta , phi ) ;

	vec_3_t y = r * spherical_to_cartesian( theta , phi ) ;

    double const v_res = norm_2( x - y ) ;

    // std::cout << "vector residual: " << v_res << '\n' ;

    v_res_max = std::max( v_res , v_res_max ) ;

  }
  
  std::cout << "max residual: "
	        << v_res_max << '\n' ;
  


  std::cout << "Testing quaternion rotation wrt Euler angle rotation.\n" ;

  v_res_max = 0 ;

  for( int i = 0 ; i < 10000 ; ++i ) {

    vec_3_t r = random_unit_vector() ;

    euler_angles<> const ea( std::rand() , std::rand() , std::rand() ) ;

    vec_3_t v1 = yaw_pitch_roll( ea ) * r ;
    vec_3_t v2 = rotation( ea , r ) ;

    double const v_res = norm_2( v1 - v2 ) ;

    // std::cout << "vector residual: " << v_res << '\n' ;

    v_res_max = std::max( v_res , v_res_max ) ;

  }

  std::cout << "max value (should be around double epsilon = "
            << std::numeric_limits< double >::epsilon() 
            << "): "
	        << v_res_max << '\n' ;

  std::cout << "Testing quaternion -> dcm conversion.\n" ;

  v_res_max = 0 ;

  for( int i = 0 ; i < 10000 ; ++i ) {

	vec_3_t      const v = random_unit_vector()     ;
	quaternion<> const q = random_unit_quaternion() ;

	vec_3_t const v1 = rotation( q , v ) ;
	vec_3_t const v2 = make_dcm( q ) * v ;
    
	double const v_res = norm_2( v1 - v2 ) ;
    
	v_res_max = std::max( v_res , v_res_max ) ;

  }
  
  std::cout << "max value (should be around double epsilon = "
            << std::numeric_limits< double >::epsilon() 
            << "): "
	        << v_res_max << '\n' ;


  std::cout << "Testing DCM -> euler angles -> quaternion -> DCM.\n" ;
  
  v_res_max = 0 ;

  for( int i = 0 ; i < 10000 ; ++i ) {
	
	quaternion<> const q = random_unit_quaternion() ;
	mat_3_t const C1 = make_dcm( q ) ;
	euler_angles<> const ea = make_euler_angles( C1 ) ;
	quaternion<> const q1 = make_quaternion( ea ) ;
	mat_3_t const C2 = make_dcm( q1 ) ;

	v_res_max = std::max( max_abs_norm( C2 - C1 ) , v_res_max ) ;
 
  }
  
  std::cout << "max residual: "
	        << v_res_max << '\n' ;
   


  std::cout << "Testing quaternion <-> Euler angles conversion.\n" ;

  double q_res_max          = 0 ;
  double change_psi_max     = 0 ;
  double change_psi_max_dcm = 0 ;

  for( int i = 0 ; i < 10000 ; ++i ) {

    quaternion  <> const q1 = random_unit_quaternion(    ) ;
    euler_angles<> const ea = make_euler_angles     ( q1 ) ;
    quaternion  <>       q2 = make_quaternion       ( ea ) ; 

    {
      double const res = std::min( abs( q1 - q2 ) , abs( q1 + q2 ) ) ;
      q_res_max = std::max( res , q_res_max ) ;
    }

	{
	  double const psi = ea.psi ;
	  change_psi( q2 , psi + 1 ) ;
	  change_psi( q2 , psi     ) ;

	  {
		double const res = std::min( abs( q1 - q2 ) , abs( q1 + q2 ) ) ;
		change_psi_max = std::max( res , change_psi_max ) ;
	  }
	}

	{ 
	  mat_3_t const C = make_dcm( q1 ) ;

	  mat_3_t C_mod = C ;
	  euler_angles<> const ea = make_euler_angles( C ) ;
	  change_psi( C_mod , ea.psi + 1 ) ;
	  change_psi( C_mod , ea.psi     ) ;

	  change_psi_max_dcm = 
		std::max( max_abs_norm( C_mod - C ) , change_psi_max_dcm ) ;
	}

  }

  std::cout << "max conversion residual:   " 
	        << q_res_max      
			<< '\n' ;
  std::cout << "max change_psi() residual: " 
	        << change_psi_max 
			<< '\n' ;
  std::cout << "max change_psi() residual (DCM): " 
	        << change_psi_max_dcm
			<< '\n' ;

  std::cout 
    << "Testing rotation_quaternion():\n" 
    << "n times 2pi / n about random axis.\n" ;

  for( long n = 7 ; n <= 10000000 ; n *= 2 ) {

    std::cout << "n = " << n << " ... " ;
    double const theta = 2 * pi / n ;
    
    vec_3_t const a  = random_unit_vector() ;
    vec_3_t const v0 = random_unit_vector() ;
    vec_3_t       v  = v0                   ;

    quaternion<> q = rotation_quaternion( theta , a ) ;

    for( long i = 0 ; i < n ; ++i )
    { v = rotation( q , v ) ; }

    // Now v should again be v0.
    std::cout << "residual: " << norm_2( v - v0 ) << '\n' ;

  }

}
